#include "task_loop.h"
#include "main.h"
#include "stdlib.h"
#include "string.h"

#include "nrf24l01.h"
#include "bsp_hcsr.h"
#include "oled.h"
#include "motor.h"
#include "bsp_pwm.h"
#include "bsp_key.h"
#include "bsp_led.h"
#include "adc.h"

uint8_t str_show[20];
uint8_t Joy_RxBuf[20]; //
uint8_t MPU_RxBuf[10]; //

uint8_t MPU_Data[10];  //
uint8_t Con_RxBuf[10]; //

uint8_t running_mode = 1; //

uint8_t NRF_flag = 0; //
uint8_t MPU_flag = 0; //

static uint8_t run_start = 0;
static uint8_t run_mode1_flag = 0; //
static uint8_t run_mode6_flag = 0; //

extern uint8_t g_recv_buff[USART_RECEIVE_LENGTH]; // 接收缓冲区
extern uint16_t g_recv_length;					  // 接收数据长度
extern uint8_t g_recv_complete_flag;			  // 接收数据完成标志位

extern uint8_t usart5_rx_buffer[30];
extern uint8_t usart5_rx_complete_flag;
extern uint16_t usart5_rx_length;

#define calculate_deviations(a,b) abs(abs(a) - abs(b))
#define speed_limit(a) abs(a) < 150 ? 0 : a

void Battery_Display()
{
    uint16_t battery_voltage = adcValue * 9900 / 4096;
    uint8_t show_str[4];
    // printf("Battery voltage=%1d.%1d\n", battery_voltage/1000,(battery_voltage/100)%10);
    sprintf(show_str, "%1d.%1dV", battery_voltage / 1000, (battery_voltage / 100) % 10);
    OLED_ShowString(95, 0, show_str, 16, 1);
    OLED_Refresh();
}

void control_KEY_Proc(void)
{
    uint8_t key_val = 0;
    key_val = KEY_Read();
    if (key_val != uckey)
        uckey = key_val;
    else
        key_val = 0;

    switch (key_val)
    {
    case KEYM_Press_Flag:
        // LEDM^=1;

        break;
    case KEYS_Press_Flag:
        // LEDS^=1;
        run_start = !run_start;
        LEDS = run_start;
        break;
    default:
        break;
    }
}
void print_array(uint8_t str[])
{
	for (int i = 0; str[i] != '\0'; i++)
		printf("str[%d]:%c\n", i, str[i]);
}
void task_running_10ms(void)
{
    // mkey_val = KEY_Read();
    // if(mkey_val != uckey)
    // 	uckey=mkey_val;
    // else
    // 	mkey_val = 0;
    // KEY_Proc();
    // Control();
}

void task_running_1s(void)
{
    adc_get_val();
    LEDM ^= 1;

    if (adcValue > 2200 && adcValue < 2305)
    {
        beep_on();
    }
    else
    {
        beep_off();
    }
    OLED_Clear_pager(7);
    printf("Running mode :%d\n", running_mode);
    switch (running_mode)
    {
    case 1:
        OLED_ShowString(40, 50, "static", 16, 1);
        if(!run_mode1_flag)
             car_stop(1); 

        break;

    case 2:
        OLED_ShowString(40, 50, "Wireless", 16, 1);
        break;

    case 3:
        OLED_ShowString(40, 50, "Gravity", 16, 1);
        break;

    case 4:
        OLED_ShowString(40, 50, "Evadible", 16, 1);
        break;

    case 5:
        OLED_ShowString(40, 50, "Follow", 16, 1);
        break;
    case 6:
        OLED_ShowString(40, 50, "Track", 16, 1);
        break;
    default:
        break;
    }
    
    Battery_Display();
}
void hcsr_control(void)
{
    OLED_Clear();
    OLED_ShowString(20, 10, "HCSR CONTROL", 16, 1);
    OLED_Refresh();
    delay_1ms(2000);

    uint16_t m_distance = 0;
    uint8_t key_val = 0;
    uint8_t running = 1;
    uint8_t running_check = 0;
    while (1)
    {
        running_check++;
        // sprintf(str_show, "Running",m_distance)
        // LED_ShowString(40,30,str_show,16,1);
        OLED_ShowNum(50, 40, m_distance, 3, 16, 1);
        OLED_Refresh();
        if (running_check < 10)
        {
            running_check++;
        }
        else
            running_check = 0;
        key_val = KEY_Read();
        if (1 == key_val)
        {
            break;
        }
        else if (2 == key_val)
        {
            running = !running;
        }
        if (10 == running_check)
        {

            m_distance = HCSR04_Get_Distance();

            if (running)
            {
                if (m_distance < 45)
                {
                    car_front(100);
                }
                else if (m_distance > 60)
                {
                    car_back(100);
                }
                else
                {
                    car_stop(0);
                }
            }

            else
            {
                car_stop(0);
                LEDS = !LEDS;
                // delay_1ms(500);
            }
        }
    }
}

int Map(int val, int in_min, int in_max, int out_min, int out_max)
{
    return (int)(val - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void Bluetooth_Mode(void)
{
    static uint16_t speed = 300;
    if (g_recv_complete_flag)
    {
        g_recv_complete_flag = 0;
        printf("g_recv_length:%d ", g_recv_length); // 打印接收的数据长度
        printf("g_recv_buff:%s\r\n", g_recv_buff);  // 打印接收的数据

        switch (g_recv_buff[0])
        {
        case '1':
        
            car_front(speed); // 前进
            break;
        case '2':
            car_right(speed); // 右转
            break;
        case '3':
            car_left(speed); // 左转
            break;
        case '4':
            car_stop(1); // 停止
            break;
        case '5':
            car_back(speed); // 后退
            break;
        case '6':
            led_l_on(); // 开灯
            led_r_on();
            break;
        case '7':
            led_l_off(); // 关灯
            led_r_off();
            break;
        case '8':
            speed = 300; // 慢速
            break;
        case '9':
            speed = 400; // 中速
            break;
        case 'M':
            speed = 500; // 快速
            break;
        default:
            //						printf("指令错误！！！\r\n");
            break;
        }
        memset(g_recv_buff, 0, g_recv_length); // 清空数组
        g_recv_length = 0;

        run_mode1_flag=1;
    }
    else
    {

    }
}

/**
 * @brief : Wireless handle remote control mode
 *
 *
 *
 * @return ** void
 */
void Wireless_Mode(void)
{
    int Joy_Lx = 50, Joy_Ly = 50, Joy_Rx = 50, Joy_Ry = 50;
    int Map_Lx, Map_Ly, Map_Rx, Map_Ry;
    int pwm1, pwm2, pwm3, pwm4;

    NRF24L01_RX_Mode();

    if (NRF24L01_RxPacket(Joy_RxBuf) == 0)
    {
        Joy_Lx = (Joy_RxBuf[2] - '0') * 10 + (Joy_RxBuf[3] - '0');
        Joy_Ly = (Joy_RxBuf[5] - '0') * 10 + (Joy_RxBuf[6] - '0');

        Joy_Rx = (Joy_RxBuf[9] - '0') * 10 + (Joy_RxBuf[10] - '0');
        Joy_Ry = (Joy_RxBuf[12] - '0') * 10 + (Joy_RxBuf[13] - '0');

        Map_Lx = Map(Joy_Lx, 10, 90, -127, 127);
        Map_Ly = Map(Joy_Ly, 10, 90, -127, 127);
        Map_Rx = Map(Joy_Rx, 10, 90, -127, 127);
        Map_Ry = Map(Joy_Ry, 10, 90, -127, 127);

        pwm1 = -Map_Ly - Map_Lx - Map_Ry - Map_Rx;
        pwm2 = -Map_Ly + Map_Lx - Map_Ry + Map_Rx;
        pwm3 = -Map_Ly - Map_Lx - Map_Ry + Map_Rx;
        pwm4 = -Map_Ly + Map_Lx - Map_Ry - Map_Rx;

        pwm1 = Map(pwm1, -127, 127, -499, 499);
        pwm2 = Map(pwm2, -127, 127, -499, 499);
        pwm3 = Map(pwm3, -127, 127, -499, 499);
        pwm4 = Map(pwm4, -127, 127, -499, 499);

        if (pwm1 < 20 && pwm1 > -20)
            pwm1 = 0;
        if (pwm2 < 20 && pwm2 > -20)
            pwm2 = 0;
        if (pwm3 < 20 && pwm3 > -20)
            pwm3 = 0;
        if (pwm4 < 20 && pwm4 > -20)
            pwm4 = 0;

        if (pwm1 > 499)
            pwm1 = 499;
        if (pwm2 > 499)
            pwm2 = 499;
        if (pwm3 > 499)
            pwm3 = 499;
        if (pwm4 > 499)
            pwm4 = 499;

        if (pwm1 < -499)
            pwm1 = -499;
        if (pwm2 < -499)
            pwm2 = -499;
        if (pwm3 < -499)
            pwm3 = -499;
        if (pwm4 < -499)
            pwm4 = -499;
        // printf("pwm1=%d\npwm2=%d\npwm3=%d\npwm4=%d\n \n",\
        // pwm1,pwm2,pwm3,pwm4);
        if (1 == run_start)
        {
            if (pwm1 >= 0)
            {
                motor_LQ_front(speed_limit(pwm1));
            }
            else if (pwm1 < 0)
            {
                motor_LQ_back(abs(speed_limit(pwm1)));
            }
            if (pwm2 >= 0)
            {
                motor_RH_front(speed_limit(pwm2));
            }
            else if (pwm2 < 0)
            {
                motor_RH_back(abs(speed_limit(pwm2)));
            }

            if (pwm3 >= 0)
            {
                motor_RQ_front(speed_limit(pwm3));
            }
            else if (pwm3 < 0)
            {
                motor_RQ_back(abs(speed_limit(pwm3)));
            }

            if (pwm4 >= 0)
            {
                motor_LH_front(speed_limit(pwm4));
            }
            else if (pwm4 < 0)
            {
                motor_LH_back(abs(speed_limit(pwm4)));
            }
        }
        else
            car_stop(0);

        if (pwm3 >= 300 && pwm1 <= -300)
        {
            led_l_on();
            led_r_off();
        }
        else if (pwm1 >= 300 && pwm3 <= -300)
        {
            led_l_off();
            led_r_on();
        }
        else
        {
            led_l_off();
            led_r_off();
        }

        if (Joy_RxBuf[0] == 'L' && Joy_RxBuf[3] == 'R')
        {
            // Motion_State(ON);
            NRF_flag = 1;
        }
        // printf((char*)Joy_RxBuf);
        // printf("\n");
    }
    else
    {
    }

    control_KEY_Proc();
    delay_ms(10);
}

/**
 * @brief :Ultrasonic obstacle avoidance mode
 *
 *
 *
 * @return ** void
 */
void Evadible_Mode(void)
{
    uint16_t m_distance = 0;
    m_distance = HCSR04_Get_Distance();
    car_front(150);
    // printf("m_distance=%d\n",m_distance);
    if (m_distance < 30)
    {
        car_back(150);
        // delay_1ms(500);
        // car_stop(0);
        delay_1ms(300);
        car_left(200);
        delay_1ms(500);
    }
}
/**
 * @brief :The gyroscope MPU6050 attitude controls the car movement
 *
 *
 * @return ** void
 */
void Gravity_Mode(void)
{
    int MPU_pitch = 50, MPU_roll = 50;
    int Map_pitch, Map_roll;
    int pwm1, pwm2, pwm3, pwm4;
    NRF24L01_RX_Mode();

    if (NRF24L01_RxPacket(MPU_RxBuf) == 0)
    {
        MPU_pitch = (MPU_RxBuf[1] - '0') * 10 + (MPU_RxBuf[2] - '0');
        MPU_roll = (MPU_RxBuf[4] - '0') * 10 + (MPU_RxBuf[5] - '0');

        Map_pitch = Map(MPU_pitch, 10, 90, -127, 127);
        Map_roll = Map(MPU_roll, 10, 90, -127, 127);

        pwm1 = Map_pitch + Map_roll;
        pwm2 = Map_pitch - Map_roll;
        pwm3 = Map_pitch + Map_roll;
        pwm4 = Map_pitch - Map_roll;

        pwm1 = Map(pwm1, -127, 127, -499, 499);
        pwm2 = Map(pwm2, -127, 127, -499, 499);
        pwm3 = Map(pwm3, -127, 127, -499, 499);
        pwm4 = Map(pwm4, -127, 127, -499, 499);

        if (pwm1 < 20 && pwm1 > -20)
            pwm1 = 0;
        if (pwm2 < 20 && pwm2 > -20)
            pwm2 = 0;
        if (pwm3 < 20 && pwm3 > -20)
            pwm3 = 0;
        if (pwm4 < 20 && pwm4 > -20)
            pwm4 = 0;

        if (pwm1 > 499)
            pwm1 = 499;
        if (pwm2 > 499)
            pwm2 = 499;
        if (pwm3 > 499)
            pwm3 = 499;
        if (pwm4 > 499)
            pwm4 = 499;

        if (pwm1 < -499)
            pwm1 = -499;
        if (pwm2 < -499)
            pwm2 = -499;
        if (pwm3 < -499)
            pwm3 = -499;
        if (pwm4 < -499)
            pwm4 = -499;
        //  printf("pwm1=%d\npwm2=%d\npwm3=%d\npwm4=%d\n \n",\
        //  pwm1,pwm2,pwm3,pwm4);
        if (1 == run_start)
        {
            if (pwm1 >= 0)
            {
                motor_LQ_front(speed_limit(pwm1));
            }
            else if (pwm1 < 0)
            {
                motor_LQ_back(abs(speed_limit(pwm1)));
            }
            if (pwm2 >= 0)
            {
                motor_RQ_front(speed_limit(pwm2));
            }
            else if (pwm2 < 0)
            {
                motor_RQ_back(abs(speed_limit(pwm2)));
            }

            if (pwm3 >= 0)
            {
                motor_LH_front(speed_limit(pwm3));
            }
            else if (pwm3 < 0)
            {
                motor_LH_back(abs(speed_limit(pwm3)));
            }

            if (pwm4 >= 0)
            {
                motor_RH_front(speed_limit(pwm4));
            }
            else if (pwm4 < 0)
            {
                motor_RH_back(abs(speed_limit(pwm4)));
            }
        }
        else
            car_stop(0);

        if (pwm3 >= 300 && pwm1 <= -300)
        {
            led_l_on();
            led_r_off();
        }
        else if (pwm1 >= 300 && pwm3 <= -300)
        {
            led_l_off();
            led_r_on();
        }
        else
        {
            led_l_off();
            led_r_off();
        }


        if (MPU_RxBuf[0] == 'M' && MPU_RxBuf[1] == 'P' && MPU_RxBuf[2] == 'U')
        {
            MPU_flag = 1;
        }
    }
    else
    {
        // printf("Receive failed !\n");
    }
    control_KEY_Proc();
    delay_ms(10);
}
/**
 * @brief :
 *
 * @return ** void
 */
void Follow_Mode(void)
{
    uint16_t m_distance = 0;
    m_distance = HCSR04_Get_Distance();
    // car_front(300);
    if (m_distance <= 40)
    {
        car_back(300);
    }
    else if (m_distance <= 70 && m_distance >= 50)
    {
        car_front(300);
    }
    else
    {
        car_stop(0);
    }
}

void Track_Mode(void)
{
    int8_t deviation = 0, last_deviation = 1;
    // if(1==run_mode6_flag)
    // {
        usart5_send_string("TX");
    //     // printf("send tx\n");
    //     run_mode6_flag=2;
    // }
        
    if (usart5_rx_complete_flag)
    {
        usart5_rx_complete_flag = 0;
        printf("g_recv_length:%d ", usart5_rx_length);  // 打印接收的数据长度
        printf("g_recv_buff:%s\r\n", usart5_rx_buffer); // 打印接收的数据
        if (usart5_rx_buffer[0] == '-')
        {
            // printf("\n负数\n");
            deviation = -1 * ((usart5_rx_buffer[1] - '0') * (usart5_rx_length==4 ? 10:1) + (usart5_rx_buffer[2] - '0'))* (usart5_rx_length==4 ? 1:0);
        }
        else
        {
            deviation = ((usart5_rx_buffer[0] - '0') * (usart5_rx_length==3 ? 10:1) + (usart5_rx_buffer[1] - '0'))* (usart5_rx_length==3 ? 1:0);
            // printf("\n正数\n");
        }
        printf("deviation=%d\n", deviation);

        memset(usart5_rx_buffer, 0, usart5_rx_length); // 清空数组
        usart5_rx_length = 0;
        if (calculate_deviations(deviation, last_deviation) > 5)
        {
            if (deviation > 10)
            {
                car_right(deviation * 10);
            }
            else if (deviation < -10)
            {
                car_left(abs(deviation) * 10);
            }
        }

        else
        {
            car_stop(0);
        }
        last_deviation = deviation;
    }
}
void Control(void)
{
    NRF24L01_RX_Mode();
    if (NRF24L01_RxPacket(Con_RxBuf) == 0 && Con_RxBuf[0] != ' ')
    {
        if (Con_RxBuf[0] == 'L' && Con_RxBuf[1] == 'Y') // 锟斤拷锟斤拷模式
        {
            running_mode = 1;
            run_mode6_flag=2;
        }
        else if (Con_RxBuf[0] == 'L' && Con_RxBuf[1] == 'X') //
        {
            running_mode = 2;
        }
        else if (Con_RxBuf[0] >= 'P' && Con_RxBuf[3] <= 'R') //
        {
            running_mode = 3;
           
        }
        else if (Con_RxBuf[0] == 'B' && Con_RxBuf[1] == 'Z') //
        {
            running_mode = 4;
        }
        else if (Con_RxBuf[0] == 'G' && Con_RxBuf[1] == 'S') //
        {
            running_mode = 5;
        }
        else if (Con_RxBuf[0] == 'C' && Con_RxBuf[1] == 'D') //
        {
            running_mode = 6;
            // printf("CD\n");
            run_mode6_flag=1;
        }
        delay_ms(10);
        // printf("mode=%d\n",running_mode);
    }
    else //
    {
        running_mode = 1;
        
    }
    control_KEY_Proc();
    delay_ms(10);

    if (running_mode == 1)
    {
        if (1 == run_start)
            Bluetooth_Mode();
        else
            car_stop(0);
        
    }

    else if (running_mode == 2)
    {
        while (1)
        {
            Wireless_Mode(); //
            if (NRF_flag == 1)
            {
                car_stop(0);
                led_l_off();
                led_r_off();
                running_mode = 1;
                car_stop(0);
                break;
            }
        }
    }
    else if (running_mode == 3)
    {
        while(1)
        {
        	Gravity_Mode();
        	if(MPU_flag==1)
        	{
        		running_mode=1;
                car_stop(0);
        		break;
        	}
        }
    }
    else if (running_mode == 4)
    {
        run_mode1_flag=0;
        if (1 == run_start)
            Evadible_Mode();
            
        else
            car_stop(0);
    }
    else if (running_mode ==5)
    {
        run_mode1_flag=0;
        if (1 == run_start)
            Follow_Mode();
        else
            car_stop(0);
    }
    else if (running_mode == 6)
    {
        run_mode1_flag=0;
        if (1 == run_start)
            Track_Mode();
        else
            car_stop(0);
    }

    if(6 !=running_mode && 1==run_mode6_flag )
    {
        run_mode6_flag=0;
        printf("mode:%d\n",running_mode);
        usart5_send_string("RX");
         printf("send rx\n");
    }
    MPU_flag = 0;
    NRF_flag = 0;
}
